import asyncio
import serial_asyncio
import serial
import threading
import time
import glob
import json
import serial.tools.list_ports


class Motor:
    def __init__(self):
        self.speed = 0
        self.angle = 0
        self.limit = 12.0
        self.transport = None

    def __repr__(self) -> str:
        return json.dumps(
            {
                "speed": self.speed,
                "angle": self.angle,
            }
        )


state = {
    "A": Motor(),
    "B": Motor(),
    "C": Motor(),
    "D": Motor(),
}


class MotorA(asyncio.Protocol):
    def __init__(self):
        pass

    def connection_made(self, transport):
        self.transport = transport
        state["A"].transport = transport
        print("port A opened", transport)
        transport.serial.rts = False  # You can manipulate Serial object via transport
        # transport.write(b"Hello, World!\n")  # Write serial data via transport
        transport.serial.reset_input_buffer()

    def data_received(self, data):
        # print('data received', repr(data))
        lines = data.decode().split("\r\n")
        # print( v1, v2)
        if len(lines) >= 1 and "\t" in lines[0]:
            pass
        else:
            return
        speed = lines[0].split("\t")
        try:
            v = float(speed[0])
            if abs(v) < 60:
                state["A"].speed = v
            state["A"].angle = float(speed[1])
            # state["A"].current = float(speed[2])
        except Exception as e:
            print(e)


class MotorB(asyncio.Protocol):
    def __init__(self):
        pass

    def connection_made(self, transport):
        self.transport = transport
        state["B"].transport = transport
        print("port B opened", transport)
        transport.serial.rts = False  # You can manipulate Serial object via transport
        # transport.write(b"Hello, World!\n")  # Write serial data via transport
        transport.serial.reset_input_buffer()

    def data_received(self, data):
        # print('data received', repr(data))
        lines = data.decode().split("\r\n")
        # print( v1, v2)
        if len(lines) >= 1 and "\t" in lines[0]:
            pass
        else:
            return
        speed = lines[0].split("\t")
        try:
            v = float(speed[0])
            if abs(v) < 60:
                state["B"].speed = v
            state["B"].angle = float(speed[1])
            # state["A"].current = float(speed[2])
        except Exception as e:
            print(e)


class MotorC(asyncio.Protocol):
    def __init__(self):
        pass

    def connection_made(self, transport):
        self.transport = transport
        state["C"].transport = transport
        print("port C opened", transport)
        transport.serial.rts = False  # You can manipulate Serial object via transport
        # transport.write(b"Hello, World!\n")  # Write serial data via transport
        transport.serial.reset_input_buffer()

    def data_received(self, data):
        # print('data received', repr(data))
        lines = data.decode().split("\r\n")
        # print( v1, v2)
        if len(lines) >= 1 and "\t" in lines[0]:
            pass
        else:
            return
        speed = lines[0].split("\t")
        try:
            v = float(speed[0])
            if abs(v) < 60:
                state["C"].speed = v
            state["C"].angle = float(speed[1])
            # state["A"].current = float(speed[2])
        except Exception as e:
            print(e)


class MotorD(asyncio.Protocol):
    def __init__(self):
        pass

    def connection_made(self, transport):
        self.transport = transport
        state["D"].transport = transport
        print("port D opened", transport)
        transport.serial.rts = False  # You can manipulate Serial object via transport
        # transport.write(b"Hello, World!\n")  # Write serial data via transport
        transport.serial.reset_input_buffer()

    def data_received(self, data):
        # print('data received', repr(data))
        lines = data.decode().split("\r\n")
        # print( v1, v2)
        if len(lines) >= 1 and "\t" in lines[0]:
            pass
        else:
            return
        speed = lines[0].split("\t")
        try:
            v = float(speed[0])
            if abs(v) < 60:
                state["D"].speed = v
            state["D"].angle = float(speed[1])
            # state["A"].current = float(speed[2])
        except Exception as e:
            print(e)


class BrushlessDriver:
    def __init__(self) -> None:
        self.loop = None
        self.ports = {}
        self.state = state
        self.devices = glob.glob("/dev/ttyCH9344*")
        if len(self.devices) < 4:
            print("brushless board not connected")
            return
        self.devices.sort()
        self.loop = asyncio.get_event_loop()
        A, B, C, D = self.devices
        self.ports["A"] = serial.Serial(
            port=A,
            baudrate=1e6,
            bytesize=8,
            parity=serial.PARITY_NONE,
            timeout=0.001,
            stopbits=serial.STOPBITS_ONE,
            dsrdtr=False,
        )
        self.ports["B"] = serial.Serial(
            port=B,
            baudrate=1e6,
            bytesize=8,
            parity=serial.PARITY_NONE,
            timeout=0.001,
            stopbits=serial.STOPBITS_ONE,
            dsrdtr=False,
        )
        self.ports["C"] = serial.Serial(
            port=C,
            baudrate=1e6,
            bytesize=8,
            parity=serial.PARITY_NONE,
            timeout=0.001,
            stopbits=serial.STOPBITS_ONE,
            dsrdtr=False,
        )
        self.ports["D"] = serial.Serial(
            port=D,
            baudrate=1e6,
            bytesize=8,
            parity=serial.PARITY_NONE,
            timeout=0.001,
            stopbits=serial.STOPBITS_ONE,
            dsrdtr=False,
        )
        self.start_loop()

    def run_forever(self):
        A, B, C, D = self.devices
        loop = self.loop
        coro1 = serial_asyncio.create_serial_connection(loop, MotorA, A, baudrate=1e6)
        coro2 = serial_asyncio.create_serial_connection(loop, MotorB, B, baudrate=1e6)
        coro3 = serial_asyncio.create_serial_connection(loop, MotorC, C, baudrate=1e6)
        coro4 = serial_asyncio.create_serial_connection(loop, MotorD, D, baudrate=1e6)
        transport, protocol = loop.run_until_complete(coro1)
        loop.run_until_complete(coro2)
        loop.run_until_complete(coro3)
        loop.run_until_complete(coro4)
        loop.run_forever()
        loop.close()
        print("loop ended")

    def start_loop(self):
        print("loop started")
        thread = threading.Thread(target=self.run_forever)
        thread.daemon = True
        thread.start()

    def stop_loop(self):
        if self.loop is not None:
            self.loop.stop()

    def set_speed(self, port, value):
        if port in self.ports:
            self.ports[port].write(b"V%.2f\n" % value)

    def set_angle(self, port, value):
        if port in self.ports:
            self.ports[port].write(b"A%.2f\n" % value)

    def set_torque(self, port, value):
        if port in self.ports:
            self.ports[port].write(b"T%.2f\n" % value)

    def set_limit(self, port, value):
        if port in self.ports:
            self.ports[port].write(b"L%.2f\n" % value)

    def send_cmd(self, port, type, value):
        if port in self.ports:
            self.ports[port].write(bytes("%s%.2f\n" % (type, value),'utf-8'))

if __name__ == "__main__":
    motors = BrushlessDriver()
    motors.set_speed("B", 5)
    motors.set_speed("C", 5)
    time.sleep(2)
    print(state["B"])
    print(state["C"])
    motors.set_torque("B", 0)
    motors.set_torque("C", 0)
    motors.stop_loop()
    time.sleep(2)
